#include<ros/ros.h>
#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<std_msgs/UInt64.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"


//接收dds消息，并发布到ros节点


RegisterMSN *reg_sub = new RegisterMSN();

int drone_id;

std::vector<geometry_msgs::PoseStamped> drone_pose_vector(22);

ros::Subscriber ros_sub_goal, rossub_pose_drone, rossub_flag_drone_lose;
ros::Publisher rospub_goal, rospub_bizhang; 
ros::Timer bizhang_timer;

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr &pose)
{
    geometry_msgs::PoseStamped current_pose;
    std::string res_temp = pose->header.frame_id;
    
    current_pose.header.frame_id = res_temp;
    current_pose.pose.position.x = pose->pose.position.x;
    current_pose.pose.position.y = pose->pose.position.y;
    current_pose.pose.position.z = pose->pose.position.z;
    rospub_goal.publish(current_pose);

}


void pose_callback(const geometry_msgs::PoseStamped::ConstPtr &pose){
    std::string str = pose->header.frame_id;
    int drone_id_msg = atoi(str.c_str());

    drone_pose_vector[drone_id_msg-1].pose.position.x = pose->pose.position.x;
    drone_pose_vector[drone_id_msg-1].pose.position.y = pose->pose.position.y;
    drone_pose_vector[drone_id_msg-1].pose.position.z = pose->pose.position.z;

}

void lostflag_callback(const geometry_msgs::PoseStamped::ConstPtr &pose){
    std::string str = pose->header.frame_id;
    int drone_id_msg = atoi(str.c_str());

    drone_pose_vector[drone_id_msg-1].pose.position.x = -99;
    drone_pose_vector[drone_id_msg-1].pose.position.y = -99;
    drone_pose_vector[drone_id_msg-1].pose.position.z = -10;

}



void bizhang_timer_callback(const ros::TimerEvent& /*event*/) 
{
    //calculate drone_id and each drone_id_msg distance
    if(drone_id>=13)
    {
        return;
    }  
    std_msgs::Bool temp_bizhang;
    temp_bizhang.data = false;
    for(int i=0;i<drone_pose_vector.size();i++)
    {
        
        if(drone_pose_vector[i].pose.position.z<-5)
        {
            continue;
        }
        if(i==drone_id-1)
        {
            continue;
        }
        double distance_each_drone = sqrt((drone_pose_vector[i].pose.position.x-drone_pose_vector[drone_id-1].pose.position.x)*(drone_pose_vector[i].pose.position.x-drone_pose_vector[drone_id-1].pose.position.x)+
                                          (drone_pose_vector[i].pose.position.y-drone_pose_vector[drone_id-1].pose.position.y)*(drone_pose_vector[i].pose.position.y-drone_pose_vector[drone_id-1].pose.position.y)+
                                          (drone_pose_vector[i].pose.position.z-drone_pose_vector[drone_id-1].pose.position.z)*(drone_pose_vector[i].pose.position.z-drone_pose_vector[drone_id-1].pose.position.z));
        if(distance_each_drone<0.1)
        {
            //self position, distance =0
            continue;
        }
        if(abs(drone_pose_vector[i].pose.position.z-drone_pose_vector[drone_id-1].pose.position.z)>2.0)
        {
            //height safe, ignore
            continue;
        }
        double avoid_dist = 4.0;
        if(drone_id<=7)
        {
            avoid_dist = 4.0;
        }
        if(distance_each_drone<avoid_dist) //distance which stop to plan when drone_id < self drones nearby
        {
            // std::cout<<"---->distance: "<<distance_each_drone<<",i= "<<i<<",drone_id= "<<drone_id<<std::endl;
            if(i<drone_id-1)
            {
                temp_bizhang.data = true;
            }
        }
    }
    rospub_bizhang.publish(temp_bizhang);

}


int main(int argc, char* argv[]){
    reg_sub->detect_serial_number();
    ros::init(argc, argv, "SDPltDDSPosesub");
    ros::NodeHandle nodeHandle; 

    for(int i=0;i<drone_pose_vector.size();i++)
    {
        drone_pose_vector[i].pose.position.z = -10;
    }

    // 设置循环的频率
    ros::Rate loop_rate(100);

    nodeHandle.getParam("drone_id",drone_id);


    ros_sub_goal = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhou/goal_from_gcs",10,goal_callback);
    rossub_pose_drone = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhou/pose_from_drones",10,pose_callback);

    rossub_flag_drone_lose = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/zhou/lost_flag_from_gcs",10,lostflag_callback);

    
    rospub_goal = nodeHandle.advertise<geometry_msgs::PoseStamped>("/zhou/start_mission",10);
    rospub_bizhang = nodeHandle.advertise<std_msgs::Bool>("/zhou/need_stoptoavoid",10);
    bizhang_timer = nodeHandle.createTimer(ros::Duration(0.1), bizhang_timer_callback);

    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }

    nodeHandle.shutdown();
    return 0;
}
